#include "check.h"
// CHECK CLASS IMPLEMENTATION
Check::Check(){}

Check::Check(QString componentName_, CheckType type_, float thresholdValue_)
{
    componentName = componentName_;
    type = type_;
    thresholdValue = thresholdValue_;
    transformation = Eigen::Matrix4f::Identity();
}


QString Check::getComponentName()
{
    return componentName;
}

CheckType Check::getType()
{
    return type;
}

float Check::getThresholdValue()
{
    return thresholdValue;
}

Eigen::Matrix4f Check::getTransformation(Component* sourceComponent, Component* targetComponent, PointCloud<PointXYZRGB>::Ptr testCloud)
{
    if(transformation == Eigen::Matrix4f::Identity())
    {
        PointCloud<PointXYZRGB>::Ptr sourceComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr targetComponentCloud (new PointCloud<PointXYZRGB>);
        copyPointCloud(*sourceComponent->getCloud(), *sourceComponent->getIndices(), *sourceComponentCloud);
        copyPointCloud(*targetComponent->getCloud(), *targetComponent->getIndices(), *targetComponentCloud);
        // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
        IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
        registrator->setInputCloud(sourceComponentCloud);
        registrator->setInputTarget (targetComponentCloud);
        registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
        registrator->setRANSACOutlierRejectionThreshold (10);
        registrator->setRANSACIterations(100);
        registrator->setMaximumIterations (100); // 1st stop criterion
        registrator->setTransformationEpsilon (1e-9); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
        //registrator->setEuclideanFitnessEpsilon(1); // 3rd stop criterion (maximum allowed Euclidean error between two consecutive steps in the ICP loop)
        registrator->align(*testCloud);
        transformation = registrator->getFinalTransformation();
    }
    return transformation;
}

Eigen::Matrix4f Check::getTransformation(Component* sourceComponent, Component* targetComponent)
{
    if(transformation == Eigen::Matrix4f::Identity())
    {
        PointCloud<PointXYZRGB>::Ptr sourceComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr targetComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr registerdComponentCloud (new PointCloud<PointXYZRGB>);
        copyPointCloud(*sourceComponent->getCloud(), *sourceComponent->getIndices(), *sourceComponentCloud);
        copyPointCloud(*targetComponent->getCloud(), *targetComponent->getIndices(), *targetComponentCloud);
        // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
        IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
        registrator->setInputCloud(sourceComponentCloud);
        registrator->setInputTarget (targetComponentCloud);
        registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
        registrator->setRANSACOutlierRejectionThreshold (10);
        registrator->setRANSACIterations(100);
        registrator->setMaximumIterations (100); // 1st stop criterion
        registrator->setTransformationEpsilon (1e-9); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
        //registrator->setEuclideanFitnessEpsilon(1); // 3rd stop criterion (maximum allowed Euclidean error between two consecutive steps in the ICP loop)
        registrator->align(*registerdComponentCloud);
        transformation = registrator->getFinalTransformation();
    }
    return transformation;
}
